function [pL4S1,pL4S2,pL4S3]=servoL4(AngShoulder,AngLong,AngFoot)
    S1=pi-AngShoulder+pi/2;
    S2=pi+AngLong;
    S3=-AngFoot+pi/2;
    %计算单腿三个舵机角度
    pL4S1=round(S1*2000/pi+500);
    if (pL4S1<500)
        pL4S1=500;
    elseif (pL4S1>2500)
        pL4S1=2500;
    end
    %
    pL4S2=round(S2*2000/pi+500);
    if (pL4S2<500)
        pL4S2=500;
    elseif (pL4S2>2500)
            pL4S2=2500;
    end
    %
    pL4S3=round(S3*2000/pi+500);
    if (pL4S3<500)
        pL4S3=500;
    elseif (pL4S3>2500)
            pL4S3=2500;
    end
end